#include <xc.h>
#include <math.h>
#include <pic18f46k22.h>

#include "crc_source.h"
#include "mokneg.h"

#pragma config LVP = OFF
#pragma config FOSC = INTIO67                   // Internal Oscillator
//#pragma config FOSC = HSHP
#pragma config WDTEN = OFF                      // Watchdog off
#pragma config DEBUG = OFF

// DEBUG
// #define DEBUG_MODE
// #define DEBUG_BUFFERS
//unsigned char transparentMode = 0;

int main(void)
{
    initDevice();
    initUsart1();
    initUsart2();
    initTimer0();
    turnLEDsOff();

    switch(communicationState) {
        case idle: {

            break;
        }
        case receivingAsciiMessage: {
            receiveAsciiMessage();
            break;
        }
        case receivingRtuMessage: {
            receiveRtuMessage();
            break;
        }
        case convertingDataFormat: {
            switch(messageAvailability) {
                case asciiMessageIsAvailable: {
                    if( convertAsciiMessageToRtu() ) {
                        messageAvailability = noMessageIsAvailable;
                        communicationState = sendingMessage;
                    }
                    else {
                        communicationState = error;
                    }
                }
                case rtuMessageIsAvailable: {
                    if( convertRtuMessageToAscii() ) {
                        messageAvailability = noMessageIsAvailable;
                        communicationState = sendingMessage;
                    }
                    else {
                        communicationState = error;
                    }
                }
                default: {
                    errorState = unknownError;
                    communicationState = error;
                }
            }
            break;
        }
        case sendingMessage: {
            if(txBufferState == txAsciiMessageAvailable) {
                if(usart1TransmissionMode == ASCII_MODE) {
                    sendUsart1Data();
                }
                else {
                    sendUsart2Data();
                }
            }
            if( txBufferState == txRtuMessageAvailable) {
                if(usart1TransmissionMode == RTU_MODE) {
                    sendUsart1Data();
                }
                else {
                    sendUsart2Data();
                }
            }

            ENABLE_USART1_RECEPTION();
            ENABLE_USART2_RECEPTION();

            // TODO: set back everything to idle, write funciton for it.

            communicationState = idle;

            break;
        }
        case error: {

            break;
        }
    }

    // checkBuffers();

    return 0;
}

void initDevice(void)
{
    // Oscillator
    OSCCONbits.IRCF0 = 1;                   // HFINTOSC ? (16 MHz)
    OSCCONbits.IRCF1 = 1;
    OSCCONbits.IRCF2 = 1;
    OSCCONbits.SCS0 = 1;                    // System clock is the internal OSC
    OSCCONbits.SCS1 = 1;
    OSCTUNE = 0x00;                         // Default, not tuning OSC module
    OSCTUNEbits.PLLEN = 1;                  // Disable Phase Lock Loop

    // General IO
    TRISBbits.TRISB3 = 0;                   // Power LED
    PORTBbits.RB3 = 1;                      // Turn on Power LED
    
    TRISDbits.TRISD0 = 0;                   // LED2
    TRISDbits.TRISD1 = 0;                   // LED3
    TRISDbits.TRISD2 = 0;                   // LED4
    TRISDbits.TRISD3 = 0;                   // LED5

    TRISAbits.RA0 = 1;                      // DIP1 
    TRISAbits.RA1 = 1;                      // DIP2
    TRISAbits.RA2 = 1;                      // DIP3
    TRISAbits.RA3 = 1;                      // DIP4
    TRISAbits.RA4 = 1;                      // DIP5
    TRISAbits.RA5 = 1;                      // DIP6
    TRISEbits.RE0 = 1;                      // DIP7
    TRISEbits.RE1 = 1;                      // DIP8

    ANSELAbits.ANSA0 = 0;
    ANSELAbits.ANSA1 = 0;
    ANSELAbits.ANSA2 = 0;
    ANSELAbits.ANSA3 = 0;
    //ANSELAbits.ANSA4 = 0;                 // missing from header file, should report to Microchip
    ANSELAbits.ANSA5 = 0;
    ANSELEbits.ANSE0 = 0;
    ANSELEbits.ANSE1 = 0;


    // Configuring Interrupts
    INTCON = 0;                             // Disable all interrupt
    INTCON2 = 0;
    INTCON3 = 0;
    PIE1 = 0;
    PIE2 = 0;
    PIE3 = 0;
    PIE4 = 0;
    PIE5 = 0;

    INTCONbits.GIE = 1;                     // Global Interrupt Enabled
    INTCONbits.PEIE = 1;		    // Peripheral Interrupt Enabled

    readConfiguration();                    // Set up switchable functions

    return;
}

// TODO: update readConfiguration function
void readConfiguration(void)
{
    // USART1
    if(!PORTAbits.RA0) usart1TransmissionMode = RTU_MODE;
        else usart1TransmissionMode = ASCII_MODE;           //not neccesary line
    if(!PORTAbits.RA1) usart1BaudRateTemp |= 1;
    if(!PORTAbits.RA2) usart1BaudRateTemp |= 1<<1;
    switch(usart1BaudRateTemp) {
        case 0: { usart1BaudRate = B_2400;  break; }
        case 1: { usart1BaudRate = B_9600;  break; }
        case 2: { usart1BaudRate = B_19200; break; }
        case 3: { usart1BaudRate = B_38400; break; }
    }

    // USART2
    if(!PORTAbits.RA3) usart2TransmissionMode = RTU_MODE;
        else usart2TransmissionMode = ASCII_MODE;           //not neccesary line
    if(!PORTAbits.RA4) usart2BaudRateTemp |= 1;
    if(!PORTAbits.RA5) usart2BaudRateTemp |= 1<<1;
    switch(usart2BaudRateTemp) {
        case 0: { usart2BaudRate = B_2400;  break; }
        case 1: { usart2BaudRate = B_9600;  break; }
        case 2: { usart2BaudRate = B_19200; break; }
        case 3: { usart2BaudRate = B_38400; break; }
    }


    /* For more precise values, 16 bit baudrate generator can be used,
     * setting the following bits. For details, see datasheet
     *
     * SPBRGH1 = 0;                 // Not used as BRG16 = 0
     * BAUDCON1bits.BRG16 = 0;      // 8-bit Baud Rate Generator is used
    */

    // TODO: Kell ez?
    // resetDeviceArray();
}

void initTimer1(void)
{
    T1CONbits.TMR1CS0 = 0;                      // Timer1/3/5 clock source is instruction clock (FOSC/4)
    T1CONbits.TMR1CS1 = 0;
    T1CONbits.T1CKPS0 = 1;                      // Prescale 8
    T1CONbits.T1CKPS1 = 1;
    T1CONbits.T1SOSCEN = 0;                     // Dedicated Secondary oscillator circuit disabled
    T1CONbits.T1SYNC = 0;                       // Do not synchronize external clock input
    T1CONbits.T1RD16 = 1;                       // Enables register read/write of Timer1/3/5 in one 16-bit operation
    T1CONbits.TMR1ON = 0;                       // Clears Timer1/3/5

    PIE1bits.TMR1IE = 0;                        // Disable Timer 1 interrupt

    return;
}

void initTimer0 (void)
{
    T0CONbits.T08BIT = 1;                   // Timer0 is configured as an 8-bit timer/counter
    T0CONbits.T0CS = 0;                     // Internal instruction cycle clock
    T0CONbits.T0SE = 1;                     // Increment on high-to-low transition on
    T0CONbits.PSA = 0;                      // Timer0 prescaler is assigned. Timer0 clock input comes from prescaler output
    T0CONbits.T0PS2 = 1;                    // Prescale set to 256
    T0CONbits.T0PS1 = 1;
    T0CONbits.T0PS0 = 1;
    T0CONbits.TMR0ON = 0;                   // Enables Timer0
    return;
}

void turnLEDsOff(void) {
    USART1_RX_LED_OFF();
    USART1_TX_LED_OFF();
    USART2_RX_LED_OFF();
    USART2_TX_LED_OFF();
}

void interrupt myISR(void) // TODO: clear this up
{
    if(RCSTA1bits.OERR1 || RCSTA1bits.FERR1) {
        LATD0 = 1;
        for(int i=0; i<30; ++i) __delay_ms(10);
        RCSTA1bits.SPEN = 0;
        RCSTA1bits.SPEN = 1;
        LATD0 = 0;
        return;
    }
    if(PIR1bits.RC1IF) {
        // TODO: start a timer
        //startTimeoutCheckService();
        DISABLE_USART1_RECEPTION();
        DISABLE_USART2_RECEPTION();

        USART1_RX_LED_ON();
        usart1RxBuffer[usart1RxCount++] = RCREG1;
        USART1_RX_LED_OFF();

        if(usart1TransmissionMode == ASCII_MODE)
            communicationState = receivingAsciiMessage;
        else communicationState = receivingRtuMessage;

        return;
    }

    if(RCSTA2bits.OERR2 || RCSTA2bits.FERR2) {
        LATD3 = 1;
        for(int i=0; i<30; ++i) __delay_ms(10);
        RCSTA2bits.SPEN = 0;
        RCSTA2bits.SPEN = 1;
        LATD3 = 0;
        return;
    }

    if(PIR3bits.RC2IF) {
        if(usart2TransmissionMode == ASCII_MODE)
            communicationState = receivingAsciiMessage;
        else communicationState = receivingRtuMessage;
        return;
    }

    if (INTCONbits.TMR0IF) {
        INTCONbits.TMR0IE = 0;
        INTCONbits.TMR0IF = 0;
        T0CONbits.TMR0ON = 0;

        /*if(timerTest++ == 49) {
            LATD2 = ~LATD2;
            timerTest = 0;
        }*/

        messageAvailability = rtuMessageIsAvailable;
    }

    /*
    if(PIR1bits.TMR1IF) {
        PIR1bits.TMR1IF = 0;
        timeoutCount++;
        if(timeoutCount == 65000) {
            timeoutCount = 0;
            resetCommunication();
            stopTimeoutCheckService();
            modbusTxBuffer[0] = 0;
            modbusTxCount = 1;
            sendModbusData();       
        }
        else return;
        */
        /*
        timeoutCount++;
        if(timeoutCount == 24000) {             // recalculate!!!!!!!
            LATD ^=  (1<<3);
            timeoutCount = 0;
        }
        */
    /*
    }*/
    return;
}

/* TODO: uncomment, finish
void resetCommunication(void) {     // TODO: recheck and finish?
    //stopTimeoutCheckService();

    // test
    for(char l=0; l<30; ++l) {
        usart1RxBuffer[l] = 0;
        usart1RxBuffer[l] = 0;
    }
    //KNET_RX_LED_OFF();

    //Modbus RTU reset
    rtuMessageAvailable = 0;
    transmissionState = idle;
    T0CONbits.TMR0ON = 0;

    //RCSTA1bits.CREN = 0;
   // RCSTA1bits.CREN = 1;
    //RCSTA2bits.CREN = 0;
    //RCSTA2bits.CREN = 1;

    switchToModbusRx();
    switchToKnetRx();
    ENABLE_USART1_RECEPTION();
    ENABLE_USART2_RECEPTION();
    

    usart1RxCount = 0;
    usart1TxCount = 0;
    usart2RxCount = 0;
    usart2TxCount = 0;

    // DEBUG
    //DISABLE_MODBUS_RECEPTION();
    //KNET_TX_LED_ON();

#ifdef DEBUG_BUFFERS
    modbusTxBuffer[modbusTxCount++] = 'R';
    modbusTxBuffer[modbusTxCount++] = 'E';
    modbusTxBuffer[modbusTxCount++] = 'S';
    modbusTxBuffer[modbusTxCount++] = 'E';
    modbusTxBuffer[modbusTxCount++] = 'T';
    sendModbusData();
#endif

    return;
}
*/

// Todo: implement something similar
void startTimeoutCheckService(void) {
    if(T1CONbits.TMR1ON) TMR1 = 0;
    else {
        T1CONbits.TMR1ON = 1;
        PIE1bits.TMR1IE = 1;
        TMR1 = 0;
    }
    return;
}

void stopTimeoutCheckService(void) {
    PIE1bits.TMR1IE = 0;
    T1CONbits.TMR1ON = 0;
    return;
}

void switchUsart1ToTx(void)
{
    PORTDbits.RD4 = 1;
    return;
}

void switchUsart1ToRx(void) {
    PORTDbits.RD4 = 0;
    RCSTA1bits.SPEN = 0;
    RCSTA1bits.SPEN = 1;
}

void initUsart1(void)
{
    // EUSART 1

    // Setting up IO ports
    TRISCbits.RC6 = 0;                  // Set RC6 as output for TX1
    ANSELCbits.ANSC6 = 0;               // Set RC6 as Trasmitter output

    TRISCbits.RC7 = 1;                  // Set RC7 as input for RX1
    ANSELCbits.ANSC7 = 0;               // Set RC7 as Receiver input

    TRISDbits.TRISD4 = 0;               // Set RD4 as output for Modbus TX mod
    ANSELDbits.ANSD4 = 1;               // Set RD4 as LATD4 output, disable analog

    // Configuring EUSART

   // SPBRG1 = ((_XTAL_FREQ / 9600) / 64) - 1;        // baud rate
    SPBRG1 = usart1BaudRate;
    SPBRGH1 = 0;                                    // Not used as BRG16 = 0
    TXSTA1bits.BRGH = 0;                            // Low Speed mode
    BAUDCON1bits.CKTXP = 0;                         // Normal TX Data Polarity
    BAUD1CONbits.DTRXP = 0;                         // Normal RX Data Polarity
    BAUDCON1bits.BRG16 = 0;                         // 8-bit Baud Rate Generator is used
    BAUDCON1bits.ABDEN = 0;                         // Disable Autobaud detection
    BAUD1CONbits.WUE = 0;                           // Disable Wake up on receive

    RCSTA1bits.SPEN = 1;                            // Serial Port Enable bit
    RCSTA1bits.CREN = 1;                            // Enable the reception
    RCSTA1bits.RX9 = 0;                             // 8 bit mode
    RCSTA1bits.ADDEN = 0;                           // Address Detection, here don't care

    TXSTA1bits.SYNC = 0;                            // Asynchronous mode
    TXSTA1bits.TXEN = 1;                            // Transmit enabled
    TXSTA1bits.TX9 = 0;                             // 8 bit transmission mode
    TXSTA1bits.SENDB = 0;                           // don't care

    PIE1bits.TX1IE = 0;                             // Disable interrupt on transmit
    PIE1bits.RC1IE = 1;                             // Enable interrupt on receive

    return;
}

void initUsart2(void)
{
    // EUSART 2

    // Setting up IO ports
    TRISDbits.RD6 = 0;                  // Set RD6 as output for TX2
    ANSELDbits.ANSD6 = 0;               // Set RD6 as Trasmitter output

    TRISDbits.RD7 = 1;                  // Set RD7 as input for RX2
    ANSELDbits.ANSD7 = 0;               // Set RD7 as Receiver input

    TRISDbits.TRISD5 = 0;               // Set RD5 as output for K-Net TX mod
    ANSELDbits.ANSD5 = 1;               // Set RD5 as LATD5 output, disable analog

    // Configuring EUSART 2

   // SPBRG2 = ((_XTAL_FREQ / 9600) / 64) - 1;        // baud rate
    SPBRG2 = usart2BaudRate;
    SPBRGH2 = 0;                                    // Not used as BRG16 = 0
    TXSTA2bits.BRGH = 0;                            // Low Speed mode
    BAUDCON2bits.CKTXP = 0;                         // Normal TX Data Polarity
    BAUDCON2bits.DTRXP = 0;                         // Normal RX Data Polarity
    BAUDCON2bits.BRG16 = 0;                         // 8-bit Baud Rate Generator is used
    BAUDCON2bits.ABDEN = 0;                         // Disable Autobaud detection
    BAUDCON2bits.WUE = 0;                           // Disable Wake up on receive

    RCSTA2bits.SPEN = 1;                            // Serial Port Enable bit
    RCSTA2bits.CREN = 1;                            // Enable the reception
    RCSTA2bits.RX9 = 0;                             // 8 bit mode
    RCSTA2bits.ADDEN = 0;                           // Address Detection, here don't care

    TXSTA2bits.SYNC = 0;                            // Asynchronous mode
    TXSTA2bits.TXEN = 1;                            // Transmit enabled
    TXSTA2bits.TX9 = 0;                             // 8 bit transmission mode
    TXSTA2bits.SENDB = 0;                           // don't care

    PIE3bits.TX2IE = 0;                             // Disable interrupt on transmit
    PIE3bits.RC2IE = 1;                             // Enable interrupt on receive

    return;
}

void switchUsart2ToTx(void)
{
    PORTDbits.RD5 = 1;
    return;
}

void switchUsart2ToRx(void) {
    PORTDbits.RD5 = 0;
    RCSTA2bits.SPEN = 0;
    RCSTA2bits.SPEN = 1;
    return;
}

void receiveAsciiMessage(void)
{
    if(usart1RxBuffer[usart1RxCount-1] == 0x0A) {           // Is current char Modbus ASCII ending char ':' ?
        DISABLE_USART1_RECEPTION();                         // disable interrupt on receive
        DISABLE_USART2_RECEPTION();                         // TODO: recheck logic + seprate function?
        // TODO: DO I still need this line? -> probably yes
        usart1RxBuffer[usart1RxCount-1] == 9;               // dummy char

        if(validateAsciiMessage()) {                        // validating ASCII Message format
            messageAvailability = asciiMessageIsAvailable;  // Ascii message is available for convertion
            communicationState = convertingDataFormat;      // switching to converting status (ASCII -> RTU)
        }
        else {
            errorState = invalidAsciiMessage;
            communicationState = error;
        }
    }
    else {                                                  // Message is still coming in
        // PIR1bits.RCIF = 1;                               // generate interrupt for next char
        return;
    }
    
    return;
}

void receiveRtuMessage(void)
{
    waitForNextRtuCharacter();

    if(messageAvailability == rtuMessageIsAvailable) {
        DISABLE_USART1_RECEPTION();                         // disable interrupt on receive
        DISABLE_USART2_RECEPTION();
        usart1RxCount--;                                    // TODO: recheck!

        if(validateRtuMessage()) {
            communicationState = convertingDataFormat;
        }
        else {
            errorState = invalidRtuMessage;
            communicationState = error;
        }
    }

    return;
}

void waitForNextRtuCharacter(void)
{
    T0CONbits.TMR0ON = 1;                                   // Set Timer0 ON
    INTCONbits.TMR0IE = 1;                                  // ReEnable Timer0 interrupts
    TMR0 = 238;                                             // preset timeout value
    return;
}

unsigned char convertAsciiMessageToRtu(void)
{
    unsigned char i;
    unsigned char messageLenght = usart1RxCount-=5;
    unsigned int crc;

    for(i=1; i<=messageLenght; i++) {
            usart2TxBuffer[usart2TxCount++] = convertAsciiToRtu(usart1RxBuffer, i);
    }

    crc = generateCRC(usart2TxCount, usart2RxBuffer);

    usart2TxBuffer[usart2TxCount++] = crc >> 8;
    usart2TxBuffer[usart2TxCount++] = crc;

    usart1RxCount = 0;
  
    return 1;
}

unsigned char convertAsciiToRtu(unsigned char *asciiBuffer, unsigned char byteNumber)
{
    int rtuByte;

    if(asciiBuffer[byteNumber] < 58) rtuByte = (asciiBuffer[byteNumber] - 48) * 16;
    else rtuByte = (asciiBuffer[byteNumber] - 55) * 16;
    if(asciiBuffer[byteNumber+1] < 58) rtuByte += (asciiBuffer[byteNumber+1] - 48);
    else rtuByte += (asciiBuffer[byteNumber+1] - 55);

    // TODO: check endianness!
    return rtuByte & 255;
}

unsigned char convertRtuMessageToAscii(void)
{
    unsigned char i;
    unsigned char messageLenght = usart2RxCount-= 2;                // subtract the 2 byte CRC from the end
    unsigned char lrc;

    usart1TxBuffer[usart1TxCount++] = ':';

    for(i=0; i<=messageLenght; i+=2) {
            usart1TxBuffer[usart1TxCount++] = convertHexToAsciiHi(usart2RxBuffer[i]);
            usart1TxBuffer[usart1TxCount++] = convertHexToAsciiLo(usart2RxBuffer[i]);
    }

    lrc = generateLRC(usart1TxCount-1, usart1RxBuffer);

    usart1TxBuffer[usart1TxCount++] = convertHexToAsciiHi(lrc);
    usart1TxBuffer[usart1TxCount++] = convertHexToAsciiLo(lrc);

    usart1TxBuffer[usart1TxCount++] = 0x0D;
    usart1TxBuffer[usart1TxCount++] = 0x0A;

    // TODO: rethink position in code
    usart2RxCount = 0;

    return 1;
}

unsigned int generateCRC(unsigned char len, unsigned char *buffer)      	// from modbus documentation
{
	//unsigned short len = rxCount;						// quantity of bytes in message
	unsigned char uchCRCHi = 0xFF; 						// high CRC byte initialized
	unsigned char uchCRCLo = 0xFF; 						// low CRC byte initialized
	unsigned int crc;
	unsigned char uIndex, i=0; 						// will index into CRC lookup table
	while (len--) { 							// pass through message buffer
		uIndex = uchCRCHi ^ buffer[i];   				// calculate the CRC
		uchCRCHi = uchCRCLo ^ auchCRCHi[uIndex];
                uchCRCLo = auchCRCLo[uIndex];
		i++;
	}
	crc = uchCRCHi;
	crc <<= 8;
	crc += uchCRCLo;
	return crc;
}

unsigned char generateLRC(unsigned char len, unsigned char *buffer)
{
    short int i, g, h, lrc=0;
    for(i=1; i<len; i+=2) {                                                     // converting dec num in char to hex
        g = 0;
        h = 0;
        if(buffer[i] < 58) g += (buffer[i] - 48) * 16;
        else g += (buffer[i] - 55) * 16;
        if(buffer[i+1] < 58) h += (buffer[i+1] - 48);
        else h += (buffer[i+1] - 55);                                           // converting dec num in char to hex
        lrc += (g + h) & 255;
    }
    return (-(lrc)) & 255;
}

unsigned char validateAsciiMessage(void)
{
	unsigned int lrc = generateLRC(usart1RxCount-4, usart1RxBuffer);
	if(usart1RxBuffer[0] != ':') return 0;
	if(usart1RxBuffer[usart1RxCount-1] != 0x0A) return 0;
        if(convertDecToHexInAsciiHi(lrc) != usart1RxBuffer[usart1RxCount-4] ||
           convertDecToHexInAsciiLo(lrc) != usart1RxBuffer[usart1RxCount-3]) {
#ifdef DEBUG_BUFFERS
            usart1TxBuffer[usart1TxCount++] = convertDecToHexInAsciiHi(lrc);
            usart1TxBuffer[usart1TxCount++] = convertDecToHexInAsciiLo(lrc);
            usart1TxBuffer[usart1TxCount++] = usart1RxBuffer[usart1RxCount-4];
            usart1TxBuffer[usart1TxCount++] = usart1RxBuffer[usart1RxCount-3];
            sendModbusData();
#endif
                return 0;
        }
	return 1;
}

unsigned char validateRtuMessage(void)
{
	unsigned int crc;

        // DEBUG
        /*
        modbusTxCount = 0;
        modbusTxBuffer[modbusTxCount++] = 13;
        modbusTxBuffer[modbusTxCount++] = 'C';
        modbusTxBuffer[modbusTxCount++] = 'R';
        modbusTxBuffer[modbusTxCount++] = 'C';
        modbusTxBuffer[modbusTxCount++] = '=';
	modbusTxBuffer[modbusTxCount++] = modbusRxBuffer[modbusRxCount-2];
        modbusTxBuffer[modbusTxCount++] = modbusRxBuffer[modbusRxCount-1];
        modbusTxBuffer[modbusTxCount++] = 'C';
        modbusTxBuffer[modbusTxCount++] = 'R';
        modbusTxBuffer[modbusTxCount++] = 'C';
        modbusTxBuffer[modbusTxCount++] = '=';
        */
	crc = generateCRC(usart2RxCount-2, usart2RxBuffer);
	//modbusTxBuffer[modbusTxCount++] = (crc>>8);
        //modbusTxBuffer[modbusTxCount++] = (crc);

	if( usart2RxCount < 8 ) return 0;                                       // if message is smaller than minimum lenght
	if( (char)(crc>>8) != usart2RxBuffer[usart2RxCount-2] ||
            (char)crc != usart2RxBuffer[usart2RxCount-1] ) {
                return 0;                                                       // if CRC error	| reversed order!
        }
        //[modbusTxCount++] = 13;
        //sendModbusData();
	return 1;
}

void sendUsart1Data(void)
{
    unsigned char i = 0;
    switchUsart1ToTx();
    USART1_TX_LED_ON();
    for(i=0; i<usart1TxCount; ++i) {
        TXREG1 = usart1TxBuffer[i];
        while(!TXSTA1bits.TRMT1);
    }
    usart1TxCount = 0;
    USART1_TX_LED_OFF();
    switchUsart1ToRx();
    stopTimeoutCheckService(); // Timeout service stop
    //TODO: check back

    return;
}

void sendUsart2Data(void)
{
    unsigned char i = 0;

    switchUsart2ToTx();
    USART2_TX_LED_ON();
    for(i=0; i<usart2TxCount; ++i) {
        TXREG2 = usart2TxBuffer[i];
        while(!TXSTA2bits.TRMT2);
    }
    usart2TxCount = 0;
    USART2_TX_LED_OFF();
    switchUsart2ToRx();
    
    return;
}


unsigned char convertHexToAsciiHi(unsigned char num) {
    if( (num >> 4) < 10 ) return num+30;
    else return num+65;
}

unsigned char convertHexToAsciiLo(unsigned char num) {
    if( (num & 0x0F) < 10 ) return num+30;
    else return num+65;
}

char convertDecToHexInAsciiHi(int num) {
	return hexChars[ (num>>4) & 0x0F ];
}

char convertDecToHexInAsciiLo(int num) {
	return hexChars[ num & 0x0F ];
}

